#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <foxglove_msgs/msg/compressed_image.hpp>
#include <builtin_interfaces/msg/time.hpp>

class ImageRepublisher : public rclcpp::Node
{
public:
    ImageRepublisher() : Node("image_republisher")
    {
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/image", 10,
            std::bind(&ImageRepublisher::listener_callback_image, this, std::placeholders::_1));

        publisher_ = this->create_publisher<foxglove_msgs::msg::CompressedImage>("/image_compressed", 10);
    }

private:
    void listener_callback_image(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        // 创建 CompressedImage 消息
        auto image_republisher = foxglove_msgs::msg::CompressedImage();
        image_republisher.timestamp.sec = msg->header.stamp.sec;
        image_republisher.timestamp.nanosec = msg->header.stamp.nanosec;
        image_republisher.frame_id = msg->header.frame_id;
        image_republisher.format = "jpeg";
        image_republisher.data = msg->data;

        // 发布压缩后的图像消息
        publisher_->publish(image_republisher);
    }

    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
    rclcpp::Publisher<foxglove_msgs::msg::CompressedImage>::SharedPtr publisher_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImageRepublisher>());
    rclcpp::shutdown();
    return 0;
}
